// <copyright file="NavDataFileReader.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Concurrent;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction;

    /// <summary>
    /// The file reader that can read the navigation data and can publish it as navigation data messages
    /// </summary>
    public class NavDataFileReader : IFileReader
    {
        /// <summary>
        /// The file to open and read
        /// </summary>
        private string filePath;

        /// <summary>
        /// The queue of deserialized messages waiting to be published at the right time
        /// </summary>
        private BlockingCollection<AgentMessage> publishingQueue;

        /// <summary>
        /// Indicates if the reader has more messages to publish.
        /// </summary>
        private bool isActive;

        /// <summary>
        /// Camera Pose Estimation Input Forwarder used to pass the landmark ID updates to the camera pose estimation agent.
        /// </summary>
        private IForwarder<CameraPoseEstimationInputMessage> poseForwarder;

        /// <summary>
        /// The delta between message timestamps in the ground truth file and the timestamps in the sensor file, if any 
        /// Some curated sensor files use the relative timestamp. The original sensor file has both the relative and the absolute timestamp.
        /// The ground truth file has just the absolute timestamp. To solve this, we add (offline) a line prefixed with #start 
        /// to the ground truth file, relating the relative time to the absolute time.
        /// </summary>
        private long timeOffset;

        /// <summary>
        /// Keep track of message count
        /// </summary>
        private int messageCount;

        /// <summary>
        /// The expected number of goals
        /// </summary>
        private int goalCount;

        /// <summary>
        /// Initializes a new instance of the <see cref="NavDataFileReader" /> class.
        /// </summary>
        /// <param name="poseForwarder">A control message forwarder used to update the ground truth system with landmark IDs</param>
        public NavDataFileReader(IForwarder<CameraPoseEstimationInputMessage> poseForwarder)
        {
            this.poseForwarder = poseForwarder;
        }

        /// <summary>
        /// Initializes the reader
        /// </summary>
        /// <param name="filePath">The file path</param>
        /// <param name="publishingQueue">The queue where to store the messages deserialized so far</param>
        /// <returns>The recording start time, if persisted in the file</returns>
        public long Initialize(string filePath, BlockingCollection<AgentMessage> publishingQueue)
        {
            this.publishingQueue = publishingQueue;
            this.filePath = filePath;
            this.isActive = true;

            // first line in the ground truth file should have the start time of the recording
            string line = null;
            using (StreamReader fileReader = new StreamReader(this.filePath))
            {
                line = fileReader.ReadLine();

                string[] parts = line.Split();
                if (parts[0] != "#start")
                {
                    throw new Exception("NavData file is missing the start time entry.\n\r" + this.filePath);
                }

                this.timeOffset = (long)(double.Parse(parts[1]));

                line = fileReader.ReadLine();
                parts = line.Split();
                if (parts[0] != "#GoalCount")
                {
                    throw new Exception("NavData file is missing the goal count entry.\n\r" + this.filePath);
                }

                this.goalCount = int.Parse(parts[1]);
            }

            return this.timeOffset;
        }

        /// <summary>
        /// Deserializes messages and inserts them in the publishing queue.
        /// This method blocks until Stop is called or until the end of file is reached.
        /// </summary>
        public void Start()
        {
            using (StreamReader fileReader = new StreamReader(this.filePath))
            {
                NavDataMessage message = null;
                string line = this.GetNextLine(fileReader);

                // create a message in case this is the one and only entry in the file
                message = CreateNavDataMessage(line, 0);

                // loop over all entries in the file and add them to the queue.
                // need to read ahead one message to get the next landmark id
                // the queue will block when it is full according to the buffer size.
                while (!fileReader.EndOfStream && this.isActive)
                {
                    string lineNext = this.GetNextLine(fileReader);

                    // use this to parse the line and get the next landmark ID.
                    message = CreateNavDataMessage(lineNext, 0);
                    this.AddNavDataMessage(CreateNavDataMessage(line, message.LandmarkId));

                    line = lineNext;                    
                }

                if (message != null)
                {
                    if (this.messageCount == this.goalCount - 1)
                    {
                        // next landmark id of -2 indicates all goals were reached in this run
                        this.AddNavDataMessage(CreateNavDataMessage(line, -2));
                    }
                    else
                    {
                        this.AddNavDataMessage(CreateNavDataMessage(line, 0));
                    }
                }

                this.publishingQueue.CompleteAdding();
            }
        }

        /// <summary>
        /// Stops listening for messages
        /// </summary>
        public void Stop()
        {
            this.isActive = false;
        }

        /// <summary>
        /// Parses one line of the file
        /// </summary>
        /// <param name="input">The input line</param>
        /// <param name="nextLandmarkId">The next landmark id</param>
        /// <returns>A navigation data message based on the input line and next landmark id</returns>
        private static NavDataMessage CreateNavDataMessage(string input, int nextLandmarkId)
        {
            string[] parts = input.Split();
            long originatingTime = (long)(double.Parse(parts[0]));
            int landmarkId = int.Parse(parts[1]);
            double x = double.Parse(parts[2]);
            double y = double.Parse(parts[3]);
            double heading = double.Parse(parts[4]) * MathConstants.Degrees2Radians;
            int elapsedSeconds = int.Parse(parts[5]);

            return new NavDataMessage(landmarkId, nextLandmarkId, new Pose2D(x, y, heading), elapsedSeconds, originatingTime);
        }

        /// <summary>
        /// Get the next line of text while skipping over blank lines and comments (starting with #)
        /// </summary>
        /// <param name="fileReader">A file reader</param>
        /// <returns>The next line</returns>
        private string GetNextLine(StreamReader fileReader)
        {
            string line;

            line = fileReader.ReadLine();
            while (!fileReader.EndOfStream && (string.IsNullOrEmpty(line) || line[0] == '#'))
            {
                line = fileReader.ReadLine();
            }

            return line;
        }

        /// <summary>
        /// Creates a new ground truth message and adds it to the publishing queue and to the dictionary of ids
        /// </summary>
        /// <param name="message">A navigation data message</param>
        private void AddNavDataMessage(NavDataMessage message)
        {
            this.messageCount++;
            if (this.messageCount > this.goalCount)
            {
                throw new InvalidOperationException("NavDataMessage count is greter than the number of expected goals.");
            }

            this.publishingQueue.Add(message);

            // Notify the ground truth system of the first landmark id we are navigating to.
            // Future updates will be made by the navigation metric agent after each waypoint.
            // This is kind of hacky but will be removed once the grount truth system can ID landmarks automatically.
            if (this.messageCount == 1)
            {
                this.poseForwarder.Post(new CameraPoseEstimationInputMessage(message.LandmarkId), true);
            }
        }
    }
}
